#include <Max3421e.h>
#include <Usb.h>
#include <AndroidAccessory.h>

AndroidAccessory acc("Wyapples",
		     "Kitt Car",
		     "Car Arduino Board",
		     "1.0",
		     "http://www.android.com",
		     "0000000012345678");

#define PWM1 10
#define PWM2 11

#define M1A 7
#define M1B 6

#define M2A 5
#define M2B 4

void setup() {
  Serial.begin(9600);
  Serial.print("\r\nStart");
  
  carStop();
  
  acc.powerOn();
}

void loop() {
  byte msg[2];
  if (acc.isConnected()) {
    int len = acc.read(msg, sizeof(msg), 1);
    int action = int(msg[0]);
    int arg = int(msg[1]);
    
    Serial.print(len);
    Serial.print("\t");
    Serial.print(action);
    Serial.print("\t");
    Serial.print(action);
    Serial.print("\r\n");
    if (len > 0) {
        if (action == 1)
            goAhead(25, arg);
        else if (action == 2)
            goBack(25, arg);
        else if (action == 3)
            turnLeft(25, arg);
        else if (action == 4)
            turnRight(25, arg);
        
        msg[0] = 1;    
        acc.write(msg, 1);
    }
  } else {
    carStop();
  }
}

void genPWM(int speed) {
  analogWrite(PWM1, speed);
  analogWrite(PWM2, speed);
}

void carStop() {
  digitalWrite(M1A, LOW);
  digitalWrite(M1B, LOW);
  digitalWrite(M2A, LOW);
  digitalWrite(M2B, LOW);
}

void turnRight(int speed, int time) {
  genPWM(speed);
  digitalWrite(M1A, HIGH);
  
  for(int i = 0; i < time; i++){
    delay(5);
  }
  
  carStop();
}

void turnLeft(int speed, int time) {
  genPWM(speed);
  digitalWrite(M2A, HIGH);
  
  for(int i = 0; i < time; i++){
    delay(5);
  }
  
  carStop();
}

void goAhead(int speed, int time) {
  genPWM(speed);
  digitalWrite(M1A, HIGH);
  digitalWrite(M2A, HIGH);
  
  for(int i = 0; i < time; i++){
    delay(2000);
  }
  
  carStop();
}

void goBack(int speed, int time) {
  genPWM(speed);
  digitalWrite(M1B, HIGH);
  digitalWrite(M2B, HIGH);
  
  for(int i = 0; i < time; i++){
    delay(2000);
  }
  
  carStop();
}
